#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <tf/transform_broadcaster.h>

#include <pcl_ros/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>

//#include<cv_bridge/cv_bridge.h>
#include "cv_bridge/cv_bridge.h"
#include<opencv2/opencv.hpp>
#include <nav_msgs/Odometry.h>

//#include <pcl/conversions.h>

#include <map>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/crop_box.h>

using namespace Eigen;
using namespace std;
using PointType = pcl::PointXYZI;
std::map<std::string, bool> frames_;
std::vector<std::string> frames_v_;
tf2_ros::Buffer tfbuf_;
std::map<std::string, std::string> frame_ids_;
//std::vector<std_msgs::Header> pc_accum_header_;
pcl::PointCloud<PointType>::Ptr pc_local_accum_;
int frame_num_=100118;
string frame_id_string_,pc_path_name_;
ros::Publisher cloud_pub;

Eigen::Matrix4f P2_, Trv2c_;

void pcd2bin(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,const string& out_file)
{
    //Create & write .bin file
//    ofstream bin_file(out_file.c_str(),ios::out|ios::binary|ios::app);
    ofstream bin_file(out_file.c_str());
    if(!bin_file.good()) cout<<"Couldn't open "<<out_file<<endl;

    //PCD 2 BIN
    cout << "Converting "<<endl;
    for (size_t i = 0; i < cloud->points.size (); ++i)
    {
        bin_file.write((char*)&cloud->points[i].x,3*sizeof(float));
        bin_file.write((char*)&cloud->points[i].intensity,sizeof(float));
        //cout<< 	cloud->points[i]<<endl;
    }

    bin_file.close();
}

void pc_range_handle(pcl::PointCloud<PointType>::Ptr p_pc){
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*p_pc, *p_pc, indices);
    pcl::PassThrough<PointType> pass_filter;
    pass_filter.setInputCloud (p_pc);
    pass_filter.setFilterFieldName ("z");
    pass_filter.setFilterLimits (-5, 5);
    pass_filter.setFilterLimitsNegative (false);
    pass_filter.filter (*p_pc);

    pass_filter.setInputCloud (p_pc);
    pass_filter.setFilterFieldName ("y");
    pass_filter.setFilterLimits (-10,10);
    pass_filter.setFilterLimitsNegative (false);
    pass_filter.filter (*p_pc);

    pass_filter.setInputCloud (p_pc);
    pass_filter.setFilterFieldName ("x");
    pass_filter.setFilterLimits (0, 20);
    pass_filter.setFilterLimitsNegative (false);
    pass_filter.filter (*p_pc);
}

void convertPCDBin(pcl::PointCloud<PointType>::Ptr pc_tmp,const string pc_path_name){
    pc_range_handle(pc_tmp);
    for(auto & pt:pc_tmp->points){
        pt.intensity = 0;
    }
    pcd2bin(pc_tmp,pc_path_name);
}

void cloudCB(const vector<string>& pc_path,const vector<string>& img_path)
{
    for(int label_idx = 0; label_idx < pc_path.size(); label_idx++){
        frame_id_string_=(__gnu_cxx::__to_xstring<string>(&std::vsnprintf, 4 * sizeof(int),
                                                        "%06d", frame_num_));
        pcl::PointCloud<PointType>::Ptr pc_tmp(new pcl::PointCloud<PointType>);//转成pcl点云
        pcl::io::loadPCDFile<PointType>(pc_path[label_idx], *pc_tmp);

        string root_path = "/media/st/data_fast/keti/";
        // string tranin_or_test="training";
        string tranin_or_test="envalute/han_label";
        string dataset_name = "0826";
        cout<<tranin_or_test<<endl;

        // pc_path_name_=root_path+dataset_name+"/"+tranin_or_test+"/velodyne/"+frame_id_string_+".bin";

        cv::Mat img= cv::imread(img_path[label_idx]);
        string img_path_name = root_path+dataset_name+"/"+tranin_or_test+"/image_2/"+frame_id_string_+".png";
        cv::imwrite(img_path_name,img);

        string pc_path_name = root_path+dataset_name+"/"+tranin_or_test+"/velodyne/"+frame_id_string_+".pcd";
        pcl::io::savePCDFile(pc_path_name,*pc_tmp);

        frame_num_++;
    }
}

vector<string> getFiles(string cate_dir)
{
    vector<string> files; //存放文件名

    DIR *dir;
    struct dirent *ptr;
    char base[1000];

    if ((dir = opendir(cate_dir.c_str())) == NULL)
    {
        perror("Open dir error...");
        exit(1);
    }

    while ((ptr=readdir(dir)) != NULL)
	{
		if(strcmp(ptr->d_name,".")==0 || strcmp(ptr->d_name,"..")==0)    ///current dir OR parrent dir
            continue;
        else if(ptr->d_type == 8)    ///file
			//printf("d_name:%s/%s\n",basePath,ptr->d_name);
			files.push_back(ptr->d_name);
		else if(ptr->d_type == 10)    ///link file
			//printf("d_name:%s/%s\n",basePath,ptr->d_name);
			continue;
		else if(ptr->d_type == 4)    ///dir
		{
			files.push_back(ptr->d_name);
			/*
		        memset(base,'\0',sizeof(base));
		        strcpy(base,basePath);
		        strcat(base,"/");
		        strcat(base,ptr->d_nSame);
		        readFileList(base);
			*/
		}
	}
	closedir(dir);
 
	//排序，按从小到大排序
	sort(files.begin(), files.end());
	return files;
}

int main(int argc, char* argv[])
{
    string pc_root_path = "/media/st/data_fast/keti/0826/envalute/testing/handmake_label/velodyne/";
    string img_root_path = "/media/st/data_fast/keti/0826/envalute/testing/handmake_label/image_2/";
    vector<string> pc_path = getFiles(pc_root_path);
    vector<string> img_path = getFiles(img_root_path);
    
    for (auto &path : pc_path){
        path = pc_root_path+"/"+path;
        cout<<path<<endl;
    }
    for (auto &path : img_path){
        path = img_root_path+"/"+path;
        cout<<path<<endl;
    }

    cloudCB(pc_path,img_path);

    return 0;
}
